package api;

import lejos.nxt.remote.NXTCommand;
import lejos.pc.comm.NXTComm;
import lejos.pc.comm.NXTCommException;
import lejos.pc.comm.NXTCommFactory;
import lejos.pc.comm.NXTInfo;

import java.io.IOException;

public class RobotAPIWrapperImpl implements RobotAPIWrapper
{
  private final int STOPPED = 0;
  private final int FORWARD = 1;
  private final int BACKWARD = 2;
  private final int LEFT = 3;
  private final int RIGHT = 4;

  private double speed;
  boolean ready;
  boolean turnInPlace;
  private NXTCommand nxtCommand;
  private int state;

  public RobotAPIWrapperImpl(String nxtName, boolean turnInPlace)
  {
    this.turnInPlace = turnInPlace;
    try
    {
      ready = false;
      speed = 50;
      state = STOPPED;
      NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
      NXTInfo[] arr = nxtComm.search(nxtName, NXTCommFactory.BLUETOOTH);
      if(arr.length > 0)
      {
        System.out.println("Connecting to " + arr[0].name);
        nxtComm.open(arr[0]);

        nxtCommand = new NXTCommand();
        nxtCommand.setNXTComm(nxtComm);
        System.out.println("Is open: " + nxtCommand.isOpen());
        ready = true;
      }
      else
        System.err.println(arr.length);
    }
    catch(NXTCommException e)
    {
        System.out.println("bad news bears");
        e.printStackTrace();
    }
  }

  public boolean isReady()
  {
    return ready;
  }

  public void forward()
  {
    try
    {
      nxtCommand.setOutputState(0xFF, (byte)(int)speed, 0x07, 0x01, 0, 0x00, 0);
      state = FORWARD;
    }
    catch(IOException e){}
  }

  public void backward()
  {
    try
    {
      nxtCommand.setOutputState(0xFF, (byte)(int)-speed, 0x07, 0x01, 0, 0x00, 0);
      state = BACKWARD;
    }
    catch(IOException e){}
  }

  public void stop()
  {
    try
    {
      nxtCommand.setOutputState(0xFF, (byte)0, 0x07, 0x01, 100, 0x00, 0);
      state = STOPPED;
    }
    catch(IOException e){}
  }

  public void left()
  {
    try
    {
      nxtCommand.setOutputState(0xFF, (byte)0, 0x07, 0x01, 100, 0x00, 0);
      state = STOPPED;
      nxtCommand.setOutputState(0x01, (byte)(int)(speed / 4.0), 0x07, 0x01, 100, 0x00, 0);
      if(turnInPlace)
        nxtCommand.setOutputState(0x00, (byte)(int)-(speed / 4.0), 0x07, 0x01, 100, 0x00, 0);
      state = LEFT;
    }
    catch(IOException e){}
  }

  public void right()
  {
    try
    {
      nxtCommand.setOutputState(0xFF, (byte)0, 0x07, 0x01, 100, 0x00, 0);
      state = STOPPED;
      nxtCommand.setOutputState(0x00, (byte)(int)(speed / 4.0), 0x07, 0x01, 100, 0x00, 0);
      if(turnInPlace)
        nxtCommand.setOutputState(0x01, (byte)(int)-(speed / 4.0) , 0x07, 0x01, 100, 0x00, 0);
      state = RIGHT;
    }
    catch(IOException e){}
  }

  private void boundSpeed()
  {
    if(speed > 100)
      speed = 100;
    if(speed < 1)
      speed = 1;
  }

  public void setSpeed(int perdec)
  {
    speed = perdec * 10;
    boundSpeed();
    continueMovement();
  }

  public void speedUp(int perdec)
  {
    speed = speed * (perdec + 10) / 10.0;
    boundSpeed();
    continueMovement();
  }

  public void slowDown(int perdec)
  {
    speed = speed * (10 - perdec) / 10.0;
    boundSpeed();
    continueMovement();
  }

  private void continueMovement()
  {
    switch(state)
    {
      case FORWARD:
        forward();
        break;
      case BACKWARD:
        backward();
        break;
      case LEFT:
        left();
        break;
      case RIGHT:
        right();
        break;
    }
  }
}

